
///////////////////////////////////////////////////////////////////////////////
//
//  Copyright (c) 2009, Adam Kubach
//  All rights reserved.
//  BSD License: http://www.opensource.org/licenses/bsd-license.html
//
///////////////////////////////////////////////////////////////////////////////

#include "Minerva/Core/Data/Camera.h"
#include "Minerva/Core/TileEngine/LandModel.h"

#include "Usul/Math/Constants.h"

#include <cmath>

using namespace Minerva::Core::Data;


///////////////////////////////////////////////////////////////////////////////
//
//  Constructor.
//
///////////////////////////////////////////////////////////////////////////////

Camera::Camera() : BaseClass(),
  _longitude ( 0.0 ),
  _latitude ( 0.0 ),
  _altitude ( 0.0 ),
  _heading ( 0.0 ),
  _tilt ( 0.0 ),
  _roll ( 0.0 )
{
}


///////////////////////////////////////////////////////////////////////////////
//
//  Destructor.
//
///////////////////////////////////////////////////////////////////////////////

Camera::~Camera()
{
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set the longitude.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::longitude ( double lon )
{
  Guard guard ( this );
  _longitude = lon;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Get the longitude.
//
///////////////////////////////////////////////////////////////////////////////

double Camera::longitude() const
{
  Guard guard ( this );
  return _longitude;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set the latitude.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::latitude ( double lat )
{
  Guard guard ( this );
  _latitude = lat;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Get the latitude.
//
///////////////////////////////////////////////////////////////////////////////

double Camera::latitude() const
{
  Guard guard ( this );
  return _latitude;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set the altitude.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::altitude ( double altitude )
{
  Guard guard ( this );
  _altitude = altitude;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Get the altitude.
//
///////////////////////////////////////////////////////////////////////////////

double Camera::altitude() const
{
  Guard guard ( this );
  return _altitude;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set the heading.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::heading ( double heading )
{
  Guard guard ( this );
  _heading = heading;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Get the heading.
//
///////////////////////////////////////////////////////////////////////////////

double Camera::heading() const
{
  Guard guard ( this );
  return _heading;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set the tilt.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::tilt ( double tilt )
{
  Guard guard ( this );
  _tilt = tilt;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Get the tilt.
//
///////////////////////////////////////////////////////////////////////////////

double Camera::tilt() const
{
  Guard guard ( this );
  return _tilt;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set the roll.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::roll ( double roll )
{
  Guard guard ( this );
  _roll = roll;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Get the roll.
//
///////////////////////////////////////////////////////////////////////////////

double Camera::roll() const
{
  Guard guard ( this );
  return _roll;
}


///////////////////////////////////////////////////////////////////////////////
//
//  Set from a view matrix.
//
///////////////////////////////////////////////////////////////////////////////

void Camera::viewMatrix ( const Matrix& matrix, Minerva::Core::TileEngine::LandModel* landModel )
{
  if ( 0x0 == landModel )
    return;

  Usul::Math::Vec3d eye;
  matrix.getTranslation ( eye );

  Usul::Math::Vec3d point;
  landModel->xyzToLatLonHeight ( eye[0], eye[1], eye[2], point[1], point[0], point[2] );

  this->longitude ( point[0] );
  this->latitude ( point[1] );
  this->altitude ( point[2] );

  // From bullet implmentation.
  double yaw ( Usul::Math::atan2 ( matrix ( 0, 1 ), matrix ( 0, 0 ) ) );
  double pitch ( Usul::Math::asin ( -matrix ( 0, 2 ) ) );
  double roll ( Usul::Math::atan2 ( matrix ( 1, 2 ), matrix ( 2, 2 ) ) );

  // on pitch = +/-HalfPI
  if ( ::fabs ( pitch ) == Usul::Math::PIE_OVER_2 )
  {
    if (yaw>0)
      yaw-=Usul::Math::PIE;
    else
      yaw+=Usul::Math::PIE;

    if (roll>0)
      roll-=Usul::Math::PIE;
    else
      roll+=Usul::Math::PIE;
  }

  // Roll not working, but Navigator currently doesn't modify the roll.
  this->heading ( yaw * Usul::Math::RAD_TO_DEG );
  this->tilt ( pitch * Usul::Math::RAD_TO_DEG );
  //this->roll ( roll * Usul::Math::RAD_TO_DEG );
}

///////////////////////////////////////////////////////////////////////////////
//
//  Get the view matrix.
//
///////////////////////////////////////////////////////////////////////////////

Camera::Matrix Camera::viewMatrix ( Minerva::Core::TileEngine::LandModel* landModel ) const
{
  if ( 0x0 == landModel )
    return Matrix();

  Matrix rotation ( landModel->planetRotationMatrix ( this->latitude(), this->longitude(), this->altitude(), this->heading() ) );

  Usul::Math::Vec3d translation;
  rotation.getTranslation ( translation );
  rotation.setTranslation ( Usul::Math::Vec3d ( 0.0, 0.0, 0.0 ) );

  Matrix pitchMatrix, rollMatrix;
  pitchMatrix.makeRotation ( this->tilt() * Usul::Math::DEG_TO_RAD, Usul::Math::Vec3d ( 1.0, 0.0, 0.0 ) );
  rollMatrix.makeRotation ( this->roll() * Usul::Math::DEG_TO_RAD, Usul::Math::Vec3d ( 0.0, 0.0, 1.0 ) );
  
  Matrix answer ( rotation * pitchMatrix * rollMatrix );
  answer.setTranslation ( translation );

  return answer;
}
